/**
  ******************************************************************************
  * @file           : communicate_task.cpp
  * @author         : Tessia
  * @brief          : None
  * @attention      : None
  * @date           : 2024/2/15
  ******************************************************************************
  */



#include "communicate_task.h"

extern rc_device_t g_rc;
extern uint8_t usb_rx_buf[32];
extern osSemaphoreId_t VisionUARTDataUpdateBinarySemHandle;
extern osSemaphoreId_t USBDataUpdateBinarySemHandle;
extern osSemaphoreId_t VisionCalBinarySemHandle;

extern GyroDataStruct GyroDataSave;
extern StampDataStruct TimeStampedData;

extern uint8_t vision_cal_completed;

extern uint16_t shooter_barrel_cooling_value; //每秒冷却热量
extern uint16_t shooter_barrel_heat_limit;    //热量上限

extern AimAutoStruct AimAutoData;

extern pose_cal poseCal;

extern uint8_t is_left_of_axis;
extern uint8_t is_temporary_pause;

extern uint32_t target_recognition_cnt;

extern point2f debugLocation[LOCATION_NUM];
extern point2f RMUL_location[LOCATION_NUM];
extern point2f RMUC_location[LOCATION_NUM];

extern float chassis_yaw_offset;

extern uint8_t insufficient_gold;
extern uint8_t ammunition_depleted;
extern uint8_t hp_not_full;
extern uint8_t hp_full;
extern uint8_t late_stage;

extern uint8_t VisionControlFlag;
extern AutoAimingParaStruct AutoAimingPara;
extern enemy_coo enemyCoo;

extern uint32_t be_hit_flag;
extern uint8_t is_need_high_speed;

extern uint8_t huart1_err;
extern uint32_t huart1_cnt;

extern sentry_state normal_state;
extern sentry_event normal_event;
extern uint8_t sentry_destination;
extern uint8_t armor_number_no_zero;

extern uint16_t judge_current_total_heat;

extern uint8_t constrained_defense_state;

uint8_t uart_vision_rx_buf[64];
uint8_t uart_vision_tx_buf[64];
uint8_t robot_data_update = 0;


uint16_t count=0;
uint16_t len_buff=0;
uint32_t CameraReceiveCount = 0;
float CameraReceiveDelta = 0;
float chassis_yaw_deg = 0;
chassis_info chassisInfo;
radar_info radarInfo = {};
radar_info_tran *radar_info_t;
enemy_robot_coo radar_station_data[8] = {};  //其中0,6为空


coo_info_tran cooInfoTran = {};
coo_info cooInfo = {3.92, 3.15, 1};

speed_info speedInfo = {0,0};

judge_info_0 judgeInfo0={.last_HP = 600,
                         .current_HP = 600,
                         };
judge_info_1 judgeInfo1={};
judge_info_2 judgeInfo2={};
judge_info_3 judgeInfo3={};
judge_HP_info judgeHpInfo={};
judge_HP_info judgeHpInfoLast={};

armor_chassis_info armorChassisInfo;
yaw_and_decision_info YawAndDecisionInfo;
sentry_decision_info sentry_decision;

match_type matchType;

twins_receive_info_tran twins_receive_tran={};
twins_receive_info twins_receive={};

twins_tran_info twins_tran;

sentry_to_robot sentryToRobot;
enemy_coo_tran sentry_to_radar;
aerial_operator_data aerialOperatorData;        //化为车体坐标系

uint8_t enemy_aim_enable[12] = {0,1,1,1,1,1,1,0,1,1,1,1};
uint16_t enemy_HP[12] = {0};

uint16_t restored_HP = 0;            //已恢复生命值，RMUL用

uint16_t outpost_fire_cnt = 0;          //前哨站模式打弹量限制
uint8_t exchange_ammo_request = 0;              //兑换发弹量请求
uint8_t remote_exchange_ammo_request = 0;       //远程兑换发弹量请求
uint8_t exchange_ammo_request_last = 0;
uint8_t remote_exchange_ammo_request_last = 0;

uint16_t projectile_allowance_17mm_last = 400;

uint8_t mini_camera_1_enemy_data[12] = {};

uint16_t healing_spot_ammo_accumulation = 0;     //补血点累计弹量

uint8_t radar_connect = 0;

uint16_t attack_outpost_time = NORMAL_ATTACK_OUTPOST_TIME;
uint16_t attack_engineer_time = NORMAL_ATTACK_ENGINEER_TIME;

#if FORCE_NOT_SPIN
uint8_t is_spin = 0;
#else
uint8_t is_spin = 1;
#endif


uint8_t outpost_not_spin = 0;

float YAW_COMPENSATION = 0;


void vision_transmit(){
#if DEBUG_VISION_TRANS
    if(DEBUG_VISION_FLAG){
        //Get Gyro and encoder value
        GyroAndEncoderDataGet();
        TimeStampedData.EncoderData = EncoderDataSave;
        TimeStampedData.GyroData = GyroDataSave;
        TimeStampedData.time_stamp=osKernelGetTickCount();//Increase
        time1 = TimeStampedData.time_stamp;
        //USB TxMessage
        TransmitDataTest(&VisionTransmitTest,UsbTxBuf);
        len_buff = sizeof(UsbTxBuf);
        CDC_Transmit_FS(UsbTxBuf,USB_Buff_Size);		//·???
//            HAL_GPIO_WritePin(GPIOB,GPIO_PIN_15,GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOB,GPIO_Pin,GPIO_PIN_SET);
        DEBUG_VISION_FLAG = 0;
    }
    else{
//            HAL_GPIO_WritePin(GPIOB,GPIO_PIN_15,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(GPIOB,GPIO_Pin,GPIO_PIN_RESET);
        DEBUG_VISION_FLAG = 0;
    }
#else
    /*
     * 自瞄7
     *
     */
    CameraReceiveDelta = (float)(1.0f / DWT_GetDeltaT(&CameraReceiveCount));
    //Get Gyro and encoder value
    GyroDataGet();
    TimeStampedData.GyroData = GyroDataSave;
    TimeStampedData.time_stamp=osKernelGetTickCount();//Increase
    //USB TxMessage
    TransmitData(&VisionTransmit, uart_vision_tx_buf);
    len_buff = sizeof(uart_vision_tx_buf);
    HAL_UART_Transmit_DMA(&huart1, (uint8_t *)uart_vision_tx_buf, sizeof(uart_vision_tx_buf));
    osDelay(1);
#endif
}

void can_chassis_callback(can_device_receive *can_receive, uint8_t *data){
    auto tmp = (chassis_info_tran *)data;
    chassisInfo.yaw_angle = tmp->yaw_angle;
    chassisInfo.is_flip_over = tmp->is_flip_over;
    chassis_yaw_deg = chassisInfo.yaw_angle * 360.f;
}

void can_rc_callback(can_device_receive *can_receive, uint8_t *data){
    auto rc_tran = (rc_info_tran *)data;
    rc_update(&g_rc, data);
    g_rc.is_lost = rc_tran->is_lost;
}


/**
 * @param can_receive
 * @param data
 * 这里下面的两个赋值是因为main.c中已经重定义了回调函数，所以需要复用以下
 */
uint16_t stage_remain_time_last_0 = 0;
void can_judge0_callback(can_device_receive *can_receive, uint8_t *data){
    judge_info_0_tran *judgeInfo0Tran = (judge_info_0_tran *)data;

    judgeInfo0.mains_power_shooter_output = judgeInfo0Tran->mains_power_shooter_output;
    judgeInfo0.HP_deduction_reason = judgeInfo0Tran->HP_deduction_reason;
    judgeInfo0.be_hit = judgeInfo0Tran->be_hit;
    judgeInfo0.game_progress = judgeInfo0Tran->game_progress;
    judgeInfo0.shooter_barrel_cooling_value = judgeInfo0Tran->shooter_barrel_cooling_value;
    judgeInfo0.stage_remain_time = judgeInfo0Tran->stage_remain_time;
    judgeInfo0.last_HP = judgeInfo0.current_HP;
    judgeInfo0.current_HP = judgeInfo0Tran->current_HP;
    judgeInfo0.hurt_armor_id = judgeInfo0Tran->hurt_armor_id;
    judgeInfo0.is_occupy_patrol_zone = judgeInfo0Tran->is_occupy_patrol_zone;
    judgeInfo0.is_occupy_healing_spot = judgeInfo0Tran->is_occupy_healing_spot;

    if(judgeInfo0.last_HP < judgeInfo0.current_HP && judgeInfo0.last_HP != 0){
        restored_HP += judgeInfo0.current_HP - judgeInfo0.last_HP;
    }
    hp_not_full = judgeInfo0.current_HP == judgeInfo1.maximum_HP ? 0 : 1;
    hp_full = judgeInfo0.current_HP == judgeInfo1.maximum_HP ? 1 : 0;

    shooter_barrel_cooling_value = judgeInfo0.shooter_barrel_cooling_value;
    outpost_not_spin = (judgeInfo0.game_progress == during_the_match && normal_state == go_attack_outpost && judgeInfo0.stage_remain_time < 240) ? 1 : 0;
    late_stage = (judgeInfo0.game_progress == during_the_match && judgeInfo0.stage_remain_time < 180) ? 1 : 0;

    healing_spot_ammo_accumulation += (judgeInfo0.game_progress == during_the_match && (judgeInfo0.stage_remain_time / 60) != (stage_remain_time_last_0 / 60) && judgeInfo0.stage_remain_time < 400) ? 100 : 0;
    stage_remain_time_last_0 = judgeInfo0.stage_remain_time;
    healing_spot_ammo_accumulation = judgeInfo0.is_occupy_healing_spot == 1 ? 0 : healing_spot_ammo_accumulation;
}

void can_judge1_callback(can_device_receive *can_receive, uint8_t *data) {
    judge_info_1_tran *judgeInfo1Tran = (judge_info_1_tran *)data;

    judgeInfo1.game_type = judgeInfo1Tran->game_type;
    judgeInfo1.shooter_barrel_heat_limit = judgeInfo1Tran->shooter_barrel_heat_limit - 10;
    judgeInfo1.is_blue = judgeInfo1Tran->is_blue;
    judgeInfo1.maximum_HP = judgeInfo1Tran->maximum_HP;
    judgeInfo1.remaining_gold_coin = judgeInfo1Tran->remaining_gold_coin;
    insufficient_gold = judgeInfo1.remaining_gold_coin < EXCHANGE_AMMO_GOLD_COIN ? 1 : 0;

    robot_data_update = 1;
    shooter_barrel_heat_limit = judgeInfo1.shooter_barrel_heat_limit;
#if MATCH_DEBUG
    matchType = RMUC;

#else
    matchType = judgeInfo1.game_type == 1 ? RMUC : RMUL;

#endif
}

void can_judge2_callback(can_device_receive *can_receive, uint8_t *data) {
    judge_info_2_tran *judgeInfo2Tran = (judge_info_2_tran *)data;

    projectile_allowance_17mm_last = judgeInfo2.projectile_allowance_17mm;
    judgeInfo2.projectile_allowance_17mm = judgeInfo2Tran->projectile_allowance_17mm;
    outpost_fire_cnt += (normal_state == go_attack_outpost && projectile_allowance_17mm_last > judgeInfo2.projectile_allowance_17mm) ? (projectile_allowance_17mm_last - judgeInfo2.projectile_allowance_17mm) : 0;
    judgeInfo2.shooter_id = judgeInfo2Tran->shooter_id;
    judgeInfo2.bullet_speed = judgeInfo2Tran->bullet_speed;
    if(judgeInfo2.shooter_id == 2 && judgeInfo2.bullet_speed > 20){
        AutoAimingPara.bullet_actual_speed = judgeInfo2.bullet_speed;
    }

    if(projectile_allowance_17mm_last < judgeInfo2.projectile_allowance_17mm){
        exchange_ammo_request = 0;
        remote_exchange_ammo_request = 0;
        ammunition_depleted = 0;
    }
}


void can_judge3_callback(can_device_receive *can_receive, uint8_t *data) {
    judge_info_3_tran *judgeInfo3Tran = (judge_info_3_tran *)data;

    judgeInfo3.shooter_17mm_barrel_heat = judgeInfo3Tran->shooter_17mm_2_barrel_heat;
    judge_current_total_heat = judgeInfo3.shooter_17mm_barrel_heat;
}


//这里没有接裁判系统进不了这个中断
uint16_t stage_remain_time_last_HP = 0;     //由于中断更新频率不一致，防止检测不到上升下降沿这里分别使用两个last
void can_HP_info0_callback(can_device_receive *can_receive, uint8_t *data){
    judge_HP_info_0 *judgeHpInfo0Tran = (judge_HP_info_0 *)data;

    judgeHpInfoLast.enemy_1_robot_HP = judgeHpInfo.enemy_1_robot_HP;
    judgeHpInfoLast.enemy_2_robot_HP = judgeHpInfo.enemy_2_robot_HP;
    judgeHpInfoLast.enemy_3_robot_HP = judgeHpInfo.enemy_3_robot_HP;
    judgeHpInfoLast.enemy_4_robot_HP = judgeHpInfo.enemy_4_robot_HP;

    judgeHpInfo.enemy_1_robot_HP = judgeHpInfo0Tran->enemy_1_robot_HP;
    judgeHpInfo.enemy_2_robot_HP = judgeHpInfo0Tran->enemy_2_robot_HP;
    judgeHpInfo.enemy_3_robot_HP = judgeHpInfo0Tran->enemy_3_robot_HP;
    judgeHpInfo.enemy_4_robot_HP = judgeHpInfo0Tran->enemy_4_robot_HP;

    enemy_aim_enable[hero] = (judgeHpInfoLast.enemy_1_robot_HP == 0 && judgeHpInfo.enemy_1_robot_HP != 0) ? 0 : enemy_aim_enable[hero];
    enemy_aim_enable[engineer] = (judgeInfo0.game_progress == during_the_match && judgeInfo0.stage_remain_time < attack_engineer_time &&
                                  stage_remain_time_last_HP >= attack_engineer_time) ? 1 : enemy_aim_enable[engineer];
    enemy_aim_enable[engineer] = (judgeHpInfoLast.enemy_2_robot_HP == 0 && judgeHpInfo.enemy_2_robot_HP != 0) ? 0 : enemy_aim_enable[engineer];
    enemy_aim_enable[infantry_3] = (judgeHpInfoLast.enemy_3_robot_HP == 0 && judgeHpInfo.enemy_3_robot_HP != 0) ? 0 : enemy_aim_enable[infantry_3];
    enemy_aim_enable[infantry_4] = (judgeHpInfoLast.enemy_4_robot_HP == 0 && judgeHpInfo.enemy_4_robot_HP != 0) ? 0 : enemy_aim_enable[infantry_4];

    enemy_HP[hero] = judgeHpInfo.enemy_1_robot_HP;
    enemy_HP[engineer] = judgeHpInfo.enemy_2_robot_HP;
    enemy_HP[infantry_3] = judgeHpInfo.enemy_3_robot_HP;
    enemy_HP[infantry_4] = judgeHpInfo.enemy_4_robot_HP;
    stage_remain_time_last_HP = judgeInfo0.stage_remain_time;
}

//这里没有接服务器进不了这个中断
uint8_t game_progress_last = match_not_start;
void can_HP_info1_callback(can_device_receive *can_receive, uint8_t *data){
    judge_HP_info_1 *judgeHpInfo1Tran = (judge_HP_info_1 *)data;

    judgeHpInfoLast.enemy_5_robot_HP = judgeHpInfo.enemy_5_robot_HP;
    judgeHpInfoLast.enemy_7_robot_HP = judgeHpInfo.enemy_7_robot_HP;
    judgeHpInfoLast.enemy_outpost_HP = judgeHpInfo.enemy_outpost_HP;
    judgeHpInfoLast.enemy_base_HP = judgeHpInfo.enemy_base_HP;

    judgeHpInfo.enemy_5_robot_HP = judgeHpInfo1Tran->enemy_5_robot_HP;
    judgeHpInfo.enemy_7_robot_HP = judgeHpInfo1Tran->enemy_7_robot_HP;
    judgeHpInfo.enemy_outpost_HP = judgeHpInfo1Tran->enemy_outpost_HP;
    judgeHpInfo.enemy_base_HP = judgeHpInfo1Tran->enemy_base_HP;

    enemy_aim_enable[infantry_5] = (judgeHpInfoLast.enemy_5_robot_HP == 0 && judgeHpInfo.enemy_5_robot_HP != 0) ? 0 : enemy_aim_enable[infantry_5];
#if ENEMY_OUTPOST_EXIST
    enemy_aim_enable[sentry] = (judgeHpInfoLast.enemy_outpost_HP != 0 && judgeHpInfo.enemy_outpost_HP == 0) ? 1 : enemy_aim_enable[sentry];
#endif
    enemy_aim_enable[sentry] = (judgeInfo0.game_progress == during_the_match && game_progress_last != during_the_match) ? 0 : enemy_aim_enable[sentry];
    enemy_aim_enable[sentry] = (judgeHpInfoLast.enemy_7_robot_HP == 0 && judgeHpInfo.enemy_7_robot_HP != 0) ? 0 : enemy_aim_enable[sentry];


#if GO_ATTACK_OUTPOST_DEBUG
    enemy_aim_enable[outpost] = 1;
#else
    enemy_aim_enable[outpost] = (judgeInfo0.stage_remain_time < attack_outpost_time && judgeInfo0.game_progress == during_the_match) ? 1 : 0;
#endif
    enemy_aim_enable[base] = judgeHpInfo.enemy_outpost_HP == 0 ? 1 : 0;

    enemy_HP[infantry_5] = judgeHpInfo.enemy_5_robot_HP;
    enemy_HP[sentry] = judgeHpInfo.enemy_7_robot_HP;
    enemy_HP[base] = judgeHpInfo.enemy_base_HP;
    enemy_HP[outpost] = judgeHpInfo.enemy_outpost_HP;
    game_progress_last = judgeInfo0.game_progress;
}

void can_HP_info2_callback(can_device_receive *can_receive, uint8_t *data){
    judge_HP_info_2 *judgeHpInfo2Tran = (judge_HP_info_2 *)data;

    judgeHpInfoLast.our_outpost_HP = judgeHpInfo.our_outpost_HP;
    judgeHpInfoLast.our_base_HP = judgeHpInfo.our_base_HP;

    judgeHpInfo.our_outpost_HP = judgeHpInfo2Tran->our_outpost_HP;
    judgeHpInfo.our_base_HP = judgeHpInfo2Tran->our_base_HP;
#if FORCE_NOT_SPIN
    is_spin = 0;
#else
#if OUR_OUTPOST_EXIST && SERVER_EXIST
    is_spin = judgeHpInfo.our_outpost_HP == 0 ? 1 : 0;
#else
    is_spin = 1;
#endif
#endif
}

void can_radar_info_callback(can_device_receive *can_receive, uint8_t *data){
    enemy_coo_tran *enemyCooTran = (enemy_coo_tran *)data;

    radar_station_data[enemyCooTran->robot_number].is_reliable = enemyCooTran->is_reliable;
    if(judgeInfo1.is_blue == 0){
        radar_station_data[enemyCooTran->robot_number].x = (float)(enemyCooTran->x) / 100.0f - X_AXIS_OFFSET;
        radar_station_data[enemyCooTran->robot_number].y = (float)(enemyCooTran->y) / 100.0f - Y_AXIS_OFFSET;
    }else{
        radar_station_data[enemyCooTran->robot_number].x = (28 - (float)(enemyCooTran->x) / 100.0f) - X_AXIS_OFFSET;
        radar_station_data[enemyCooTran->robot_number].y = (15 - (float)(enemyCooTran->y) / 100.0f) - Y_AXIS_OFFSET;
    }
}


void can_operator_info_callback(can_device_receive *can_receive, uint8_t *data){
    aerial_operator_data_tran *aerialOperatorDataTran = (aerial_operator_data_tran *)data;

    aerialOperatorData.cmd_keyboard = aerialOperatorDataTran->cmd_keyboard;
    if(judgeInfo1.is_blue == 0){
        aerialOperatorData.position_x = (float)(aerialOperatorDataTran->position_x) / 100.0f - X_AXIS_OFFSET;
        aerialOperatorData.position_y = (float)(aerialOperatorDataTran->position_y) / 100.0f - Y_AXIS_OFFSET;
    }else{
        aerialOperatorData.position_x = (28 - (float)(aerialOperatorDataTran->position_x) / 100.0f) - X_AXIS_OFFSET;
        aerialOperatorData.position_y = (15 - (float)(aerialOperatorDataTran->position_y) / 100.0f) - Y_AXIS_OFFSET;
    }
}


uint8_t chassis_yaw_stop = 0;
void communicate_can_task(void *argument) {
    can_device_receive can_rc_info(&hcan2, 0x300, can_rc_callback);
    can_device_receive can_receive_chassis_info(&hcan2, 0x301, can_chassis_callback);
    can_device_transmit can_speed_info(&hcan2, 0x302, 8, (uint8_t *)&speedInfo);
    can_device_receive can_judge_info_0(&hcan2, 0x303, can_judge0_callback);
    can_device_receive can_judge_info_1(&hcan2, 0x304, can_judge1_callback);
    can_device_transmit can_armor_chassis_info(&hcan2, 0x305, 8, (uint8_t *)&armorChassisInfo);
    can_device_receive can_HP_info_0(&hcan2, 0x306, can_HP_info0_callback);
    can_device_receive can_HP_info_1(&hcan2, 0x307, can_HP_info1_callback);
    can_device_receive can_HP_info_2(&hcan2, 0x309, can_HP_info2_callback);
    can_device_receive can_judge_info_2(&hcan2, 0x30a, can_judge2_callback);
    can_device_receive can_judge_info_3(&hcan2, 0x30f, can_judge3_callback);


    for(;;){
        taskENTER_CRITICAL();
        speedInfo.speed_x = radarInfo.x_speed;
        speedInfo.speed_y = radarInfo.y_speed;

        taskEXIT_CRITICAL();
        can_speed_info.can_send_msg();
        osDelay(2);

        taskENTER_CRITICAL();
        if(twins_receive.is_target_detected == 1 && vision_cal_completed == 1){
            armorChassisInfo.is_target_detected = 1;
            armorChassisInfo.chassis_yaw_cal = twins_receive.chassis_cal_yaw;

        }else if(twins_receive.is_target_detected == 0 && vision_cal_completed == 1){
            armorChassisInfo.is_target_detected = 1;
            armorChassisInfo.chassis_yaw_cal = poseCal.chassis_yaw;

        }else if(twins_receive.is_target_detected == 1 && vision_cal_completed == 0){
            armorChassisInfo.is_target_detected = 1;
            armorChassisInfo.chassis_yaw_cal = twins_receive.chassis_cal_yaw;
        }else{
            armorChassisInfo.is_target_detected = 0;
            armorChassisInfo.chassis_yaw_cal = 0;
        }

        if(twins_receive.is_chassis_yaw_stop == 1 || is_temporary_pause == 1 || (mini_camera_1_enemy_data[0] == 1 && VisionControlFlag == 0 && AimAutoData.AutoAim == 1)){
            armorChassisInfo.is_chassis_yaw_stop = 1;
            chassis_yaw_stop = 1;
        }else{
            armorChassisInfo.is_chassis_yaw_stop = 0;
            chassis_yaw_stop = 0;
        }

        if(radarInfo.is_spin == 0 && is_spin == 0){
            armorChassisInfo.is_spin = 0;
        }else if(radarInfo.is_spin == 1 && is_spin == 0){
            armorChassisInfo.is_spin = 0;
        }else if(radarInfo.is_spin == 0 && is_spin == 1){
            armorChassisInfo.is_spin = 0;
        }else{
            armorChassisInfo.is_spin = 1;
        }

        armorChassisInfo.is_use_capacitor = (be_hit_flag > 0 ? 1 : 0) || is_need_high_speed;
        armorChassisInfo.high_speed_spin = (be_hit_flag > 0 ? 1 : 0);
        armorChassisInfo.high_speed_translation = is_need_high_speed;
        armorChassisInfo.slope_speed_translation = !radarInfo.is_spin;
        taskEXIT_CRITICAL();
        can_armor_chassis_info.can_send_msg();
        osDelay(2);

    }
}


void communicate_chassis_yaw_task(void *argument){
    can_device_transmit can_chassis_yaw_info(&hcan2, 0x308, 8, (uint8_t *)&YawAndDecisionInfo);
    for(;;){
        taskENTER_CRITICAL();
        sentry_decision.exchange_ammo_amount += exchange_ammo_request == 1 && exchange_ammo_request_last == 0 ? 100 : 0;
        sentry_decision.remote_exchange_ammo_request_count += remote_exchange_ammo_request == 1 && remote_exchange_ammo_request_last == 0 ? 1 : 0;
        sentry_decision.is_confirm_revive = judgeInfo0.current_HP == 0 ? 1 : 0;

        YawAndDecisionInfo.chassis_yaw_real = INS.My_Yaw - chassis_yaw_offset;

        YawAndDecisionInfo.is_confirm_revive = sentry_decision.is_confirm_revive;
        YawAndDecisionInfo.is_exchange_immediate_revive = sentry_decision.is_exchange_immediate_revive;
        YawAndDecisionInfo.exchange_ammo_amount = sentry_decision.exchange_ammo_amount;
        YawAndDecisionInfo.remote_exchange_ammo_request_count = sentry_decision.remote_exchange_ammo_request_count;
        YawAndDecisionInfo.remote_exchange_health_request_count = sentry_decision.remote_exchange_health_request_count;
        taskEXIT_CRITICAL();
        can_chassis_yaw_info.can_send_msg();
        exchange_ammo_request_last = exchange_ammo_request;
        remote_exchange_ammo_request_last = remote_exchange_ammo_request;
        osDelay(1);
    }
}

void communicate_usb_receive_task(void *argument){
    static float radar_current_yaw_last = 0;
    static uint32_t radar_cnt = 0;
    osStatus_t radar_status;
    osSemaphoreAcquire(USBDataUpdateBinarySemHandle, 0);
    radar_info_t = (radar_info_tran *)usb_rx_buf;
    for(;;){
        radar_status = osSemaphoreAcquire(USBDataUpdateBinarySemHandle, 500);
        if(radar_status == osOK){
            radar_connect = 1;
            radar_cnt++;
            radarInfo.x_speed = radar_info_t->x_speed;
            radarInfo.y_speed = radar_info_t->y_speed;
            radarInfo.can_not_go = radar_info_t->can_not_go;
            radarInfo.coo_x_current = radar_info_t->coo_x_current;
            radarInfo.coo_y_current = radar_info_t->coo_y_current;
            radarInfo.radar_current_yaw = -radar_info_t->radar_yaw;
            radarInfo.is_spin = !radar_info_t->is_spin;
            radarInfo.round_cnt += (radarInfo.radar_current_yaw - radar_current_yaw_last < -180.f ? 1 : (radarInfo.radar_current_yaw - radar_current_yaw_last > 180 ? -1 : 0));
            radarInfo.radar_total_yaw = radarInfo.radar_current_yaw + radarInfo.round_cnt * 360.f;
            low_pass(radarInfo.radar_total_yaw_filtered, radarInfo.radar_total_yaw, 0.1);
            radarInfo.coo_x_correction = radar_info_t->coo_x_correction;
            radarInfo.coo_y_correction = radar_info_t->coo_y_correction;
            radarInfo.no_attack_engineer = radar_info_t->is_attack_engineer;
            radarInfo.is_not_patrol = radar_info_t->is_not_patrol;
            radarInfo.is_attack_outpost = radar_info_t->is_attack_outpost;
            radarInfo.camera_0 = radar_info_t->camera_0;
            radarInfo.camera_1 = radar_info_t->camera_1;
            radarInfo.camera_2 = radar_info_t->camera_2;
            radarInfo.camera_3 = radar_info_t->camera_3;


            attack_outpost_time = radarInfo.is_attack_outpost == 1 ? DIRECT_ATTACK_OUTPOST_TIME : NORMAL_ATTACK_OUTPOST_TIME;
            attack_engineer_time = radarInfo.no_attack_engineer == 1 ? NO_ATTACK_ENGINEER_TIME : NORMAL_ATTACK_ENGINEER_TIME;

            mini_camera_1_enemy_data[hero] = radarInfo.camera_1 >> (hero - 1) & 0x01;
            mini_camera_1_enemy_data[engineer] = radarInfo.camera_1 >> (engineer - 1) & 0x01;
            mini_camera_1_enemy_data[infantry_3] = radarInfo.camera_1 >> (infantry_3 - 1) & 0x01;
            mini_camera_1_enemy_data[infantry_4] = radarInfo.camera_1 >> (infantry_4 - 1) & 0x01;
            mini_camera_1_enemy_data[infantry_5] = radarInfo.camera_1 >> (infantry_5 - 1) & 0x01;
            mini_camera_1_enemy_data[sentry] = radarInfo.camera_1 >> (sentry - 1) & 0x01;
            mini_camera_1_enemy_data[0] = (mini_camera_1_enemy_data[hero] && enemy_aim_enable[hero]) || (mini_camera_1_enemy_data[engineer] && enemy_aim_enable[engineer])
                                          || ((mini_camera_1_enemy_data[infantry_3]) && enemy_aim_enable[infantry_3]) || (mini_camera_1_enemy_data[infantry_4] && enemy_aim_enable[infantry_4])
                                          || (mini_camera_1_enemy_data[infantry_5] && enemy_aim_enable[infantry_5]) || (mini_camera_1_enemy_data[sentry] && enemy_aim_enable[sentry]);


            YAW_COMPENSATION = radar_cnt > 500 ? radarInfo.radar_total_yaw - (INS.My_Yaw_Origin - chassis_yaw_offset) : YAW_COMPENSATION;
            radar_cnt = radar_cnt > 500 ? 0 : radar_cnt;
            radar_current_yaw_last = radarInfo.radar_current_yaw;
        }else{
            radar_connect = 0;
            radar_cnt = 0;
        }

    }
}

uint8_t usb_status;
void communicate_usb_transmit_task(void *argument){
    cooInfoTran.frame_header = 0x20;
    cooInfoTran.frame_tail = 0x21;
    for(;;){
        if(matchType == debug){
            cooInfo.coo_x = cooInfo.coo_x > debugLocation[farthest_point].x ? debugLocation[farthest_point].x : cooInfo.coo_x;
            cooInfo.coo_y = cooInfo.coo_y > debugLocation[farthest_point].y ? debugLocation[farthest_point].y : cooInfo.coo_y;
        }else if(matchType == RMUL){
            cooInfo.coo_x = cooInfo.coo_x > RMUL_location[farthest_point].x ? RMUL_location[farthest_point].x : cooInfo.coo_x;
            cooInfo.coo_y = cooInfo.coo_y > RMUL_location[farthest_point].y ? RMUL_location[farthest_point].y : cooInfo.coo_y;
        }else if(matchType == RMUC){
            if(cooInfo.coo_x > RMUC_location[farthest_point].x && cooInfo.coo_y > RMUC_location[farthest_point].y){
                cooInfo.coo_x = RMUC_location[farthest_point].x;
                cooInfo.coo_y = RMUC_location[farthest_point].y;
            }
        }

        if(cooInfo.coo_x > RMUC_location[farthest_point].x && cooInfo.coo_y > RMUC_location[farthest_point].y){
            cooInfo.coo_x = RMUC_location[farthest_point].x;
            cooInfo.coo_y = RMUC_location[farthest_point].y;
        }

        taskENTER_CRITICAL();
        cooInfoTran.coo_x = cooInfo.coo_x;
        cooInfoTran.coo_y = cooInfo.coo_y;
        cooInfoTran.stop = cooInfo.stop;
        cooInfoTran.red1blue0 = !judgeInfo1.is_blue;
        cooInfoTran.sentry_state = normal_state;
        cooInfoTran.sentry_event = normal_event;
        cooInfoTran.constrained_defence_state = constrained_defense_state;
        taskEXIT_CRITICAL();

        if(robot_data_update == 1){
            usb_status = CDC_Transmit_FS((uint8_t *)(&cooInfoTran), 32);
        }
        osDelay(1);
    }
}

//后面会有从哨兵到云台手或雷达的通信链路，所以这里单开一个任务
void communicate_robot_interaction_task(void *argument){
    can_device_receive can_radar_info(&hcan2, 0x30b, can_radar_info_callback);
    can_device_transmit can_sentry_interaction_info(&hcan2, 0x30c, 8, (uint8_t *)&sentryToRobot);
    can_device_transmit can_sentry_to_radar(&hcan2, 0x30d, 8, (uint8_t *)&sentry_to_radar);
    can_device_receive can_operator_info(&hcan2, 0x30e, can_operator_info_callback);
    for(;;){
        taskENTER_CRITICAL();
        sentryToRobot.sentry_state = normal_state;
        sentryToRobot.sentry_event = normal_event;
        sentryToRobot.sentry_destination = sentry_destination;
        taskEXIT_CRITICAL();

        taskENTER_CRITICAL();
        sentry_to_radar.robot_number = armor_number_no_zero;
        sentry_to_radar.is_reliable = VisionControlFlag;

        if(judgeInfo1.is_blue == 0){
            sentry_to_radar.x = (uint16_t)((enemyCoo.x + X_AXIS_OFFSET) * 100);
            sentry_to_radar.y = (uint16_t)((enemyCoo.y + Y_AXIS_OFFSET) * 100);
        }else{
            sentry_to_radar.x = (uint16_t)((28 - (enemyCoo.x + X_AXIS_OFFSET)) * 100);
            sentry_to_radar.y = (uint16_t)((15 - (enemyCoo.y + Y_AXIS_OFFSET)) * 100);
        }

        taskEXIT_CRITICAL();

        can_sentry_to_radar.can_send_msg();
        can_sentry_interaction_info.can_send_msg();
        osDelay(50);
    }
}

void communicate_uart_receive_task(void *argument){
    osSemaphoreAcquire(VisionUARTDataUpdateBinarySemHandle, 0);
    osSemaphoreAcquire(VisionCalBinarySemHandle, 0);
    osStatus_t status;
    for(;;){
        /**
         * 数据大的包得一直调用，不然无法进入中断
         * 或者运行一段时间自动断开
         * 无论数据大小，都得在中断里面重新调用
         */
        HAL_UARTEx_ReceiveToIdle_DMA(&huart1, uart_vision_rx_buf, sizeof(uart_vision_rx_buf));
        __HAL_DMA_DISABLE_IT(huart1.hdmarx, DMA_IT_HT);
        status = osSemaphoreAcquire(VisionUARTDataUpdateBinarySemHandle, 0);
        if(status == osOK){
            ReceiveData(uart_vision_rx_buf);
            osSemaphoreRelease(VisionCalBinarySemHandle);
        }
        osDelay(1);

    }
}

void communicate_uart_twins_transmit_task(void *argument){
    HAL_UARTEx_ReceiveToIdle_DMA(&huart6, (uint8_t *)&twins_receive_tran, sizeof(twins_receive_tran));
    __HAL_DMA_DISABLE_IT(huart6.hdmarx, DMA_IT_HT);
    for(;;){
        HAL_UARTEx_ReceiveToIdle_DMA(&huart6, (uint8_t *)&twins_receive_tran, sizeof(twins_receive_tran));
        __HAL_DMA_DISABLE_IT(huart6.hdmarx, DMA_IT_HT);
        taskENTER_CRITICAL();
        twins_tran.frame_header = 0x30;
        twins_tran.auto_aim = AimAutoData.AutoAim;
        twins_tran.is_radar_connect = radar_connect;
        twins_tran.twins_yaw = poseCal.gimbal_yaw;
        twins_tran.twins_pitch = poseCal.gimbal_pitch;
        twins_tran.radar_total_yaw = radarInfo.radar_total_yaw;
        twins_tran.is_target_detected = target_recognition_cnt > 0 ? vision_cal_completed : 0;
        twins_tran.is_left_of_axis = is_left_of_axis;
        twins_tran.camera_0_data = radarInfo.camera_0;
#if GO_ATTACK_OUTPOST_DEBUG
        twins_tran.go_attack_outpost = GO_ATTACK_OUTPOST_DEBUG;
#else
        twins_tran.go_attack_outpost = (normal_state == go_attack_outpost ? 1 : 0) + (normal_state == go_attack_outpost ? outpost_not_spin : 0);
#endif

        twins_tran.hero_aim_permit = enemy_aim_enable[hero];
        twins_tran.engineer_aim_permit = enemy_aim_enable[engineer];
        twins_tran.infantry_3_aim_permit = enemy_aim_enable[infantry_3];
        twins_tran.infantry_4_aim_permit = enemy_aim_enable[infantry_4];
        twins_tran.infantry_5_aim_permit = enemy_aim_enable[infantry_5];
        twins_tran.sentry_aim_permit = enemy_aim_enable[sentry];
        twins_tran.base_aim_permit = enemy_aim_enable[base];
        twins_tran.outpost_aim_permit = enemy_aim_enable[outpost];

        twins_tran.frame_tail = 0x31;

        taskEXIT_CRITICAL();
        HAL_UART_Transmit_DMA(&huart6, (uint8_t *)&twins_tran, sizeof(twins_tran));
        osDelay(2);
    }
}

void communicate_uart_vision_transmit_task(void *argument){
    for(;;){
        huart1_cnt++;
        huart1_err = huart1_cnt > 20 ? 1 : 0;
        vision_transmit();
    }
}